/* Includes ------------------------------------------------------------------*/

// Because of broken cmsis_os.h, we need to include arm_math first,
// otherwise chip specific defines are ommited
#define ARM_MATH_CM4
#include <arm_math.h>

#include <cmsis_os.h>
#include <math.h>
#include <stdint.h>
#include <stdlib.h>

#include <adc.h>
#include <gpio.h>
#include <main.h>
#include <spi.h>
#include <tim.h>
#include <utils.hpp>

#include "odrive_main.h"

/* Private defines -----------------------------------------------------------*/

// #define DEBUG_PRINT

/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
constexpr float adc_full_scale = static_cast<float>(1UL << 12UL);
constexpr float adc_ref_voltage = 3.3f;
/* Global variables ----------------------------------------------------------*/

// This value is updated by the DC-bus reading ADC.
// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
float vbus_voltage = 29.4f;
float ibus_ = 0.0f; // exposed for monitoring only
bool brake_resistor_armed = true;
bool brake_resistor_saturated = false;
STRUCT_CAPTURE strCapture[2] = { 0, 0, 0, 0 };


void start_general_purpose_adc(){
    //adc_software_trigger_enable(ADC0, ADC_REGULAR_CHANNEL); 
    adc_measurements_[0];
    timer_enable(TIMER8);
    timer_enable(TIMER4);
}


void vbus_sense_adc_cb(uint32_t adc_periph, bool injected){
    constexpr float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
    uint32_t ADCValue = ADC_IDATA0(adc_periph);
    vbus_voltage = ADCValue * voltage_scale;
}

// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
void low_level_fault(Motor::Error error) {
    // Disable all motors NOW!
    for (size_t i = 0; i < AXIS_COUNT; ++i) {
        safety_critical_disarm_motor_pwm(axes[i]->motor_);
        axes[i]->motor_.error_ = error;
    }
    safety_critical_disarm_brake_resistor();
}


void pwm_trig_adc_cb(uint32_t adc_periph, bool injected){
    // Ensure ADCs are expected ones to simplify the logic below
    if (!(adc_periph == ADC1 || adc_periph == ADC2)) {
        low_level_fault(Motor::ERROR_ADC_FAILED);
        return;
    };

    // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion
    // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion
    // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
    // If we are counting down, we just sampled in SVM vector 7, with zero current
    Axis& axis = injected ? *axes[0] : *axes[1];
    int axis_num = injected ? 0 : 1;
    Axis& other_axis = injected ? *axes[1] : *axes[0];
	
    bool counting_down = TIMER_CTL0(axis.motor_.hw_config_.timer) & TIMER_CTL0_DIR;
    bool current_meas_not_DC_CAL = !counting_down;

    // Check the timing of the sequencing
    if (current_meas_not_DC_CAL)
        axis.motor_.log_timing(TIMING_LOG_ADC_CB_I);
    else
        axis.motor_.log_timing(TIMING_LOG_ADC_CB_DC);

    bool update_timings = false;
    if (adc_periph == ADC1) {
        if (&axis == axes[1] && counting_down)
            update_timings = true; // update timings of M0
        else if (&axis == axes[0] && !counting_down)
            update_timings = true; // update timings of M1

        // TODO: this is out of place here. However when moving it somewhere
        // else we have to consider the timing requirements to prevent the SPI
        // transfers of axis0 and axis1 from conflicting.
        // Also see comment on sync_timers.
        if((current_meas_not_DC_CAL && !axis_num) ||
                (axis_num && !current_meas_not_DC_CAL)){
            axis.encoder_.abs_spi_start_transaction();
        }
    }
    // Load next timings for the motor that we're not currently sampling
    static unsigned char flag_next_timings_valid_nums_ = 0;
    if (update_timings) {
        if (!other_axis.motor_.next_timings_valid_) {
            if(flag_next_timings_valid_nums_++ >= 5){   
                bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_);
                if (was_armed) {
                    other_axis.motor_.error_ = Motor::ERROR_CONTROL_DEADLINE_MISSED;
                }
            } else {
                other_axis.motor_.next_timings_valid_ = false;
                safety_critical_apply_motor_pwm_timings(
                    other_axis.motor_, other_axis.motor_.next_timings_
                );
            }

        } else {
            other_axis.motor_.next_timings_valid_ = false;
            flag_next_timings_valid_nums_ = 0;
            safety_critical_apply_motor_pwm_timings(
                other_axis.motor_, other_axis.motor_.next_timings_
            );
        }
        update_brake_current();
    }
    uint32_t ADCValue;
    if (injected) {
        ADCValue = adc_inserted_data_read(adc_periph, 0);
    } else {
        ADCValue = adc_regular_data_read(adc_periph);
    }

    float current = axis.motor_.phase_current_from_adcval(ADCValue);

    if (current_meas_not_DC_CAL) {
        // ADC2 and ADC3 record the phB and phC currents concurrently,
        // and their interrupts should arrive on the same clock cycle.
        // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
        // Therefore we store the value from ADC2 and signal the thread that the
        // measurement is ready when we receive the ADC3 measurement

        // return or continue
        if (adc_periph == ADC1) {
            
            axis.motor_.current_meas_.phB = -current ;//- axis.motor_.DC_calib_.phB;
            axis.motor_.tmc_data3_  = ADCValue;
            if(axis.motor_.current_control_.is_ready == true)
                axis.motor_.current_meas_.phB -=axis.motor_.DC_calib_.phB;
            return;
        } else {
            axis.motor_.current_meas_.phC = -current ;//- axis.motor_.DC_calib_.phC;
            axis.motor_.tmc_data4_  = ADCValue;
            if(axis.motor_.current_control_.is_ready == true)
                axis.motor_.current_meas_.phC -=axis.motor_.DC_calib_.phC;
        }
        // Trigger axis thread
        axis.signal_current_meas();
    }
}

void tim_update_cb(uint32_t timer_periph){
    axes[0]->encoder_.sample_now(); 
    axes[1]->encoder_.sample_now(); 
}

void start_adc_pwm(){
    // Enable ADC and interrupts
    adc_enable(ADC0);
    adc_enable(ADC1);
    adc_enable(ADC2);
    // Warp field stabilize.
    osDelay(2);
    adc_interrupt_enable(ADC1, ADC_INT_EOC);
    adc_interrupt_enable(ADC2, ADC_INT_EOC);
    adc_interrupt_enable(ADC0, ADC_INT_EOIC);
    adc_interrupt_enable(ADC1, ADC_INT_EOIC);
    adc_interrupt_enable(ADC2, ADC_INT_EOIC);

    start_pwm(TIMER0);
    start_pwm(TIMER7);
    // TODO: explain why this offset
    sync_timers(TIMER0, TIMER7, TIMER_SMCFG_TRGSEL_ITI0, TIM_0_7_PERIOD_CLOCKS / 2 - 1 * 128);
    // Enable the update interrupt (used to coherently sample GPIO)
    timer_interrupt_enable(TIMER0,TIMER_INT_UP);
    timer_interrupt_enable(TIMER7,TIMER_INT_UP);
    // Motor output starts in the disabled state
    TIMER_CCHP(TIMER0) &= ~(TIMER_CCHP_POEN);
    TIMER_CCHP(TIMER7) &= ~(TIMER_CCHP_POEN);
    // Disarm motors and arm brake resistor
    for (size_t i = 0; i < AXIS_COUNT; ++i) {
        safety_critical_disarm_motor_pwm(axes[i]->motor_);
    }
    safety_critical_arm_brake_resistor();
}


void start_pwm(uint32_t timer_periph){
    // Init PWM
    int half_load = TIM_0_7_PERIOD_CLOCKS / 2;
    TIMER_CH0CV(timer_periph) = half_load;
    TIMER_CH1CV(timer_periph) = half_load;
    TIMER_CH2CV(timer_periph) = half_load;
    timer_enable(timer_periph);
}

void sync_timers(uint32_t tim_a, uint32_t tim_b,
                 uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) {
    // Store intial timer configs
    uint16_t MOE_store_a = TIMER_CCHP(tim_a) & (TIMER_CCHP_POEN);
    uint16_t MOE_store_b = TIMER_CCHP(tim_b) & (TIMER_CCHP_POEN);
    uint16_t CR2_store = TIMER_CTL1(tim_a);
    uint16_t SMCR_store = TIMER_SMCFG(tim_b);
    // Turn off output
    TIMER_CCHP(tim_a) &= ~(TIMER_CCHP_POEN);
    TIMER_CCHP(tim_b) &= ~(TIMER_CCHP_POEN);
    // Disable both timer counters
    TIMER_CTL0(tim_a) &= ~(TIMER_CTL0_CEN);
    TIMER_CTL0(tim_b) &= ~(TIMER_CTL0_CEN);
    // Set first timer to send TRGO on counter enable
    TIMER_CTL1(tim_a) &= ~(TIMER_CTL1_MMC);
    TIMER_CTL1(tim_a) |= (TIMER_TRI_OUT_SRC_ENABLE);
    // Set Trigger Source of second timer to the TRGO of the first timer
    TIMER_SMCFG(tim_b) &= ~(TIMER_SMCFG_TRGS);
    TIMER_SMCFG(tim_b) |= (TIM_CLOCKSOURCE_ITRx);
    // Set 2nd timer to start on trigger
    TIMER_SMCFG(tim_b) &= ~(TIMER_SMCFG_SMC);
    TIMER_SMCFG(tim_b) |= TIMER_SLAVE_MODE_EVENT;
    // Dir bit is read only in center aligned mode, so we clear the mode for now
    uint16_t CMS_store_a = TIMER_CTL0(tim_a) & TIMER_CTL0_CAM;
    uint16_t CMS_store_b = TIMER_CTL0(tim_b) & TIMER_CTL0_CAM;
    TIMER_CTL0(tim_a) &= ~(TIMER_CTL0_CAM);
    TIMER_CTL0(tim_b) &= ~(TIMER_CTL0_CAM);
    // Set both timers to up-counting state
    TIMER_CTL0(tim_a) &= ~(TIMER_CTL0_DIR);
    TIMER_CTL0(tim_b) &= ~(TIMER_CTL0_DIR);
    // Restore center aligned mode
    TIMER_CTL0(tim_a) |= (CMS_store_a);
    TIMER_CTL0(tim_b) |= (CMS_store_b);
    // set counter offset
    TIMER_CNT(tim_a) = count_offset;
    TIMER_CNT(tim_b) = 0;
    // Start Timer a
    TIMER_CTL0(tim_a) |= (TIMER_CTL0_CEN);
    // Restore timer configs
    TIMER_CTL1(tim_a) = CR2_store;
    TIMER_SMCFG(tim_b) = SMCR_store;
    // restore output
   TIMER_CCHP(tim_a)  |= MOE_store_a;
   TIMER_CCHP(tim_b)  |= MOE_store_b;
}
void safety_critical_arm_motor_pwm(Motor& motor){
    uint32_t mask = cpu_enter_critical();
    motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS;
    cpu_exit_critical(mask);
}
bool safety_critical_disarm_motor_pwm(Motor& motor){
    uint32_t mask = cpu_enter_critical();
	
    bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED;
	
    motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
	
    TIMER_CCHP(motor.hw_config_.timer) &=~(TIMER_CCHP_POEN);
    if(motor.axis_->axis_num_==1)
	    gpio_bit_toggle(GPIOA,GPIO_PIN_1);
    cpu_exit_critical(mask);
	
    return was_armed;
}
void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]){
    uint32_t mask = cpu_enter_critical();
    /* if (!brake_resistor_armed) {
        motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
    } */
    TIMER_CH0CV(motor.hw_config_.timer)=timings[0];
    TIMER_CH1CV(motor.hw_config_.timer)=timings[1];
    TIMER_CH2CV(motor.hw_config_.timer)=timings[2];

    if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_TIMINGS) {
        // timings were just loaded into the timer registers
        // the timer register are buffered, so they won't have an effect
        // on the output just yet so we need to wait until the next
        // interrupt before we actually enable the output
        motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_UPDATE;
    } else if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_UPDATE) {
        // now we waited long enough. Enter armed state and
        // enable the actual PWM outputs.
        motor.armed_state_ = Motor::ARMED_STATE_ARMED;
        TIMER_CCHP(motor.hw_config_.timer) |=(TIMER_CCHP_POEN);  // enable pwm outputs
    } else if (motor.armed_state_ == Motor::ARMED_STATE_ARMED) {
        // nothing to do, PWM is running, all good
    } else {
        // unknown state oh no
        /* if(motor.axis_->axis_num_==1)
            gpio_bit_toggle(GPIOA,GPIO_PIN_1); */
        safety_critical_disarm_motor_pwm(motor);
    }
    cpu_exit_critical(mask);
}
void safety_critical_arm_brake_resistor(){

}
void safety_critical_disarm_brake_resistor(){

}
void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on){

}

void update_brake_current() {
    
}

void pwm_in_enc_cb(uint8_t m0_m1,int channel, uint32_t timestamp) {

	if(m0_m1 == 1u) {
	    int gpio_num = (channel);
        printf("%d\r\n",timestamp);
		if(gpio_num == 1){
		    if(timestamp <= 4200){
		        strCapture[0].usPeriod = timestamp + 1u;
		    }
			else{
				strCapture[0].ucFinishFlag = 2u;
			}
		}else if(gpio_num == 2){
		    if(strCapture[0].ucFinishFlag!=2)
{
				
			    if(timestamp <= 4200){
			        strCapture[0].usCtr = timestamp + 1u;
			        strCapture[0].ucFinishFlag = 1u;
			    }
				else{
					strCapture[0].usPeriod = 0;
					strCapture[0].usCtr = 0;
				    strCapture[0].ucFinishFlag = 0u;
				}
		    }
			else{
				strCapture[0].usPeriod = 0;
				strCapture[0].usCtr = 0;
				strCapture[0].ucFinishFlag = 0u;
			}
		}
		
		if(strCapture[0].ucFinishFlag == 1u){
			  #if 1
              if(strCapture[0].usPeriod >= strCapture[0] .usCtr){	
			  	
				  if((strCapture[0] .usCtr > 16u) && (strCapture[0].usPeriod >= 24U)){
				  	  //���㹫ʽ
			          //axes[0]->encoder_.pos_abs_ = ((strCapture[0] .usCtr - 16)*calculate_lsb_times((float)strCapture[0].usPeriod)) * (4095 / ((strCapture[0].usPeriod - 24)*calculate_lsb_times((float)strCapture[0].usPeriod)));      //���ߵ�ƽ��ʵ��ʱ��ӳ�䷶Χ 0~4096
                      //����ʽ
					  axes[0]->encoder_.pos_abs_ = (4095u*(strCapture[0] .usCtr - 16u)) / (strCapture[0].usPeriod - 24u);

					  if(axes[0]->encoder_.pos_abs_ > 4095) 
					  	  axes[0]->encoder_.pos_abs_= 0;

					  axes[0]->encoder_.abs_spi_pos_updated_ = true;
				  
				  } else{
					  axes[0]->encoder_.abs_spi_pos_updated_ = false;// # encoder err
				  }
			  } else{
                   axes[0]->encoder_.abs_spi_pos_updated_ = false;// # encoder err
			  }
			  
			  #endif
			  
			 // axes[0]->encoder_.shadow_count_40ms_ = strCapture[0].usCtr;
			//  axes[0]->encoder_.shadow_count_20ms_ = strCapture[0].usPeriod;
			  
			  strCapture[0] .ucFinishFlag = 0u; 		  
		}
    }
    if(m0_m1 == 2u)  {
	    int gpio_num = (channel);

//    	if(gpio_num == 1){
//		    strCapture[1].usPeriod = timestamp + 1u;
//		}else if(gpio_num == 2){
//		    strCapture[1].usCtr = timestamp + 1u;
//		    strCapture[1].ucFinishFlag = 1u;
//		}
		if(gpio_num == 1){
		    if(timestamp <= 4200){
		        strCapture[1].usPeriod = timestamp + 1u;
		    }
			else{
				strCapture[1].ucFinishFlag = 2u;
			}
		}else if(gpio_num == 2){
		    if(strCapture[1].ucFinishFlag!=2)
{
				
			    if(timestamp <= 4200){
			        strCapture[1].usCtr = timestamp + 1u;
			        strCapture[1].ucFinishFlag = 1u;
			    }
				else{
					strCapture[1].usPeriod = 0;
					strCapture[1].usCtr = 0;
				    strCapture[1].ucFinishFlag = 0u;
				}
		    }
			else{
				strCapture[1].usPeriod = 0;
				strCapture[1].usCtr = 0;
				strCapture[1].ucFinishFlag = 0u;
			}
		}
		if(strCapture[1].ucFinishFlag == 1u){	
			  #if 1
              if(strCapture[1].usPeriod >= strCapture[1] .usCtr){	
			  	
				  if((strCapture[1] .usCtr > 16u) && (strCapture[1].usPeriod >= 24U)){

					  axes[1]->encoder_.pos_abs_ = (4095u*(strCapture[1].usCtr - 16u)) / (strCapture[1].usPeriod - 24u);

					  if(axes[1]->encoder_.pos_abs_ > 4095) 
					  	  axes[1]->encoder_.pos_abs_= 0;

					  axes[1]->encoder_.abs_spi_pos_updated_ = true;
				  
				  } else{
					  axes[1]->encoder_.abs_spi_pos_updated_ = false;
				  }
			  } else{
                   axes[1]->encoder_.abs_spi_pos_updated_ = false;
			  }
			  #endif

			  //axes[1]->encoder_.shadow_count_40ms_ = strCapture[1].usCtr;
			  //axes[1]->encoder_.shadow_count_20ms_ = strCapture[1].usPeriod;
			  
			  strCapture[1] .ucFinishFlag = 0u; 		  
		}
    }
}


